/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/publicInterfaceSrc/ros_callbacks.cpp
 * @Description  : 实现ROS回调函数。这些函数处理AGV的里程计、模式、事件、任务、状态等相关信息，
 *                 并通过ROS话题和服务进行通信。主要功能包括获取AGV的里程计数据、执行和清空转向命令、
 *                 处理事件和模式变化、处理订单和瞬时动作请求、发布AGV的状态和速度信息等。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:11:05
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "ros_callbacks.h" 
#include "tools.h"
#include "public_kernel.h"
#include "public_helper.h"
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <agv_msgs/msg/state.hpp>
#include "timestamp.h"
#include "check_manager.h"
#include "version.h"
#include "tf2/utils.h"

extern PublicAgvData PublicData;
extern WorkStatusRecord VhcStatus;
extern std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>> AgvVelPub; // 全局发布自动时速度(cmd_vel)
std::mutex g_mode_mutex;   // 全局控制模式锁
extern "C"
{
/**
 * @brief 获取AGV的里程计数据
 *
 * 从公开数据中获取AGV的里程计数据，并返回其引用。
 *
 * @return agv_msgs::msg::OdometryLite& 里程计数据的引用
 */
agv_msgs::msg::OdometryLite& GetOdomData() {
    return PublicData.op_data.odom_msg;
}

/**
 * @brief 获取操作数据
 *
 * 返回公共数据中的操作数据引用。
 *
 * @return OpUseTopic& 操作数据的引用
 */
OpUseTopic& GetOpData() {
    return PublicData.op_data;
}

/**
 * @brief 从里程计消息中获取速度和偏航角信息
 *
 * 从ROS话题中读取里程计消息，提取速度和偏航角信息，并存储到传入的参数中。
 *
 * @param vel 存储速度信息的对象
 * @param theta 存储偏航角信息的浮点数引用
 *
 * @return 如果成功获取数据，返回true；如果数据无效或处理过程中发生错误，返回false
 */
bool GetOdomDataHuman(Velocity& vel, float& theta) {
    const auto& op_data = GetOpData();
    auto odom_msg = op_data.odom_msg;

    // 判断数据的有效性，如果超时则为无效数据。
    if (IsTimeOut(odom_msg.header.stamp, ROSTime(), 300)) {
        DEBUG_ERROR_OUT("GetOdomData time out");
        return false;
    }

    geometry_msgs::msg::Pose data_pos = odom_msg.pose;
    geometry_msgs::msg::Twist data_speed = odom_msg.twist;
    vel.SetZero();  // 清零
    vel.vel_x_ = data_speed.linear.x;
    vel.vel_y_ = data_speed.linear.y;
    vel.angular_ = data_speed.angular.z;
    theta = 0.0f;

    try {
        theta = tf2::getYaw(data_pos.orientation);
    } catch (...) {
        DEBUG_ERROR_OUT("tf2::getYaw(odom.orientation) failed");
        return false;
    }
    return true;
}


/**
 * @brief 触发转向命令
 *
 * 该函数用于触发转向命令，并清除目标延迟触发。
 */
void SteerDirCmdTrig() {
    auto& cmd = PublicData.steer_cmd;
    cmd.Trigger();
    cmd.target_delay_once_.ClearTrigger();
    cmd.timer_readyt_delay_.ClearTrigger();
    DEBUG_OUT("SteerDirCmdTrig once");
}

/**
 * @brief 执行转向命令
 *
 * 根据输入的速度和转向方向，设置转向命令并决定是否打开转向功能。
 *
 * @param vel 车辆速度，类型为Velocity结构体
 * @param steer_dir 转向方向，整型，正值为向右转向，负值为向左转向
 * @param open 是否打开转向功能，布尔型，true表示打开，false表示关闭
 */
void DoSteerCmd(const Velocity& vel, const int& steer_dir, bool open) {
    auto& cmd = PublicData.steer_cmd;
    cmd.SetCmdVel(vel, steer_dir);
    cmd.Open(open);
    DEBUG_OUT("DoSteerCmd: set open=" << open);
}

/**
 * @brief 清空转向命令
 *
 * 该函数用于清空全局公共数据中的转向命令，以及重置转向命令的一次性延迟触发条件。
 */
void SteerCmdClear() {
    auto& cmd = PublicData.steer_cmd;
    cmd.Clear();
    cmd.target_delay_once_.ClearTrigger();
    cmd.timer_readyt_delay_.ClearTrigger();
}


// 强制延迟是否完成
bool SteerCmdDelayFinish(int time_ms)
{
    if (time_ms <= 0) {
        DEBUG_OUT("SteerCmdDelayFinish ok: ignore time delay_ms=" << time_ms);
        return true;
    }
    auto &cmd = PublicData.steer_cmd;

    // 上一次关闭，这次尝试打开一次性定时器
    if (cmd.timer_readyt_delay_.Start()) {
        DEBUG_WARN_OUT("SteerCmdDelayFinish:cmd.m_timerReadytDelay.Start");
        return false;
    }

    // 如果开启一次定时器，未达到2s 则返回未完成
    if (!cmd.timer_readyt_delay_.TriggerAlways(time_ms)) {
        DEBUG_WARN_OUT("SteerCmdDelayFinish:wait cmd.m_targetDelayOnce.Trigger " << time_ms << " ms");
        return false;
    }

    DEBUG_OUT("SteerCmdDelayFinish:ok Trigger " << time_ms << " ms");
    return true;
}

/**
 * @brief 执行转向命令
 *
 * 该函数负责执行转向命令，并检查转向命令是否成功完成。
 *
 * @return 如果转向命令成功完成，则返回 true；否则返回 false。
 */
bool FinishSteerCmd() {
    bool ret = false;
    auto& cmd = PublicData.steer_cmd;
    auto msg_fb = cmd.msg_fb_;
    const agv_msgs::msg::DoSteerDir& msg_send = cmd.msg_cmd_;
    int rx_id = msg_fb.Get().id;
    int tx_id = msg_send.id;
    bool finish = !msg_fb.Get().steering && !msg_fb.Get().chassis_not_ready;

#ifdef AGV_DEBUG_NAV
    finish = true;
    (void)(ret);
    return true;
#else
    if (rx_id != tx_id) {
        DEBUG_WARN_OUT("error: id is different; FinishSteerCmd rxId=" << rx_id << ", txId=" << tx_id);
        return ret;
    }
#endif
    // 上一次关闭，这次尝试打开一次性定时器
    if (cmd.target_delay_once_.Start()) {
        DEBUG_WARN_OUT("cmd.target_delay_once_.Start");
        return ret;
    }
    // 如果开启一次定时器，未达到2s 则返回未完成
    if (!cmd.target_delay_once_.TriggerAlways(200)) {
        DEBUG_WARN_OUT("wait cmd.target_delay_once_.Trigger 200ms");
        return ret;
    }
    // 如果舵角反馈不是最新数据 则返回未到位
    if (msg_fb.TimeOut(200)) {
        DEBUG_ERROR_OUT("steer feedback msg time out 200ms");
        return ret;
    }

    if (finish) {
        cmd.Open(false);
        // 完成后，关闭一次性定时器
        // cmd.target_delay_once_.Stop();
        DEBUG_OUT("FinishSteerCmd: set open false cmd.target_delay_once_.Stop");
    }

    DEBUG_OUT("FinishSteerCmd: finish=" << finish << ", rxId=" << rx_id << ", txId=" << tx_id << ", FeedBackCmd=" << msg_fb.Get().id);
    return finish;
}

/**
 * @brief 转向方向反馈回调函数
 *
 * 接收转向方向反馈消息，并更新公共数据中的转向命令反馈消息。
 *
 * @param data 转向方向反馈消息指针
 */
void SteerDirFeedbackCallback(const agv_msgs::msg::SteerDirFeedBack::ConstSharedPtr& data) {
    PublicData.steer_cmd.msg_fb_.Update(*data.get());
    // DEBUG_WARN_OUT("SteerDirFeedbackCallback: " << *data);
}

/**
 * @brief 发布转向命令的线程函数
 *
 * 该函数在一个单独的线程中运行，用于发布转向命令消息。
 *
 * @param nh ROS节点句柄指针
 */
void PublishSteerCmdThread(rclcpp::Node::SharedPtr node) {
    auto pub = node->create_publisher<agv_msgs::msg::DoSteerDir>("/doSteerDir", 1);
    DEBUG_OUT("PublishSteerCmdThread set start ok");
    rclcpp::Rate rate(50);
    while (rclcpp::ok()) {
        pub->publish(PublicData.steer_cmd.msg_cmd_);
        rate.sleep();
    }
}

///////////////////////////////////////////////////////////////
/**
 * @brief 事件回调函数
 *
 * 该函数用于处理AGV事件消息，并根据事件类型进行相应处理。
 *
 * @param event AGV事件消息指针
 */
void EventCallback(const agv_msgs::msg::AGVEventStatus::ConstSharedPtr& event) {
    // PublicData.curEvent = *event;
    // 如果上一次是急停事件，至少保持150ms 才可更新新的事件或无事件释放(防止频闪问题)
    auto& last_event = PublicData.cur_event;
    auto last_data = last_event.Get();

    agv_msgs::msg::AGVEvent e;
    e.ecode = event->ecode;
    e.action = event->action;
    e.type = event->type;
    e.ackreq = event->action;
    e.vellimit = event->vellimit;
    e.eventtime = event->eventtime;
    e.description = event->description;

    if (last_data.action == agv_msgs::msg::AGVEvent::ESTOP) {
        if (last_event.TimeOut(150)) {
            DEBUG_OUT("lastEvent can update: lastId=" << last_data.ecode
                << ", details=" << last_data.description << ", ctrol_mode=" << static_cast<int>(last_data.action));
            last_event.Update(e);
        } else {
            DEBUG_OUT("lastEvent cannot update: because lastId=" << last_data.ecode
                << ", details=" << last_data.description << ", ctrol_mode=" << static_cast<int>(last_data.action));
        }
    } else {
        last_event.Update(e);
    }

    static int last_id = 0xFFFF;
    if (e.ecode != last_id) {
        DEBUG_WARN_OUT("nav receive event: " << event->ecode << ", details=" << event->description << ", ctrol_mode=" << static_cast<int>(event->action));
    }
    last_id = e.ecode;
    VhcStatus.event_ = e.ecode;
}

/**
 * @brief PoseCallback 回调函数
 *
 * 当接收到PoseWithConfidence消息时，该函数会被调用。
 *
 * @param msg PoseWithConfidence类型的消息，包含AGV的位姿信息及其置信度
 */
void PoseCallback(const agv_msgs::msg::PoseWithConfidence& msg) {
    std::lock_guard<std::mutex> locker(PublicData.agv_pose_data.pose_mutex);
    PublicData.agv_pose_data.real_pose = msg;
}

/**
 * @brief 用于处理SLAM定位效果评分消息
 *
 * 该函数接收一个agv_msgs::msg::ConfidenceLevel类型的消息，并更新公共数据中的激光评分。
 *
 * @param msg 接收到的agv_msgs::msg::ConfidenceLevel类型的消息，包含SLAM的置信度信息
 */
void SlamScoreCallback(const agv_msgs::msg::ConfidenceLevel& msg) {
    PublicData.agv_pose_data.laser_score.Update(msg);
}

/**
 * @brief AGV模式回调函数
 *
 * 当接收到新的AGV模式消息时，调用此函数处理。
 *
 * @param data AGV模式消息指针
 */
void AgvModeCallback(const agv_msgs::msg::AGVMode::ConstSharedPtr& data) {
    // bit 4: manual or auto
    uint8_t state = data->mode;
    uint64_t new_work_mode = AnalysisMode(state);
    // #1 切换手自动模式
    AgvWorkMode last_mode = PublicData.cur_work_mode;

    if (static_cast<uint64_t>(last_mode) != new_work_mode) {
        std::lock_guard<std::mutex> locker(g_mode_mutex);
        SteerCmdClear();
        TryToOccurPathLost("", false);

        bool offline_old = last_mode == AgvWorkMode::mod_auto_offline;
        bool online_old = last_mode == AgvWorkMode::mod_auto_online;
        if (offline_old || online_old) {
            DEBUG_OUT("ready exit auto " << (online_old ? "online" : "offline") << " mode");
            StartNav(false, online_old);
            DEBUG_OUT("exit auto " << (online_old ? "online" : "offline") << " mode ok");
        }

        ClearNavNodeIdRecord();

        bool offline = new_work_mode == static_cast<uint64_t>(AgvWorkMode::mod_auto_offline);
        bool online = new_work_mode == static_cast<uint64_t>(AgvWorkMode::mod_auto_online);
        if (offline || online) { // #1-1 reload  map
            DEBUG_OUT("ready reload map again");
            LoadMap(); // 可能花费较长时间
            DEBUG_OUT("AgvModeCallback: thread id:" << std::this_thread::get_id());
            // #1-2 try find cur map pose  from
            if (AutoPreCheck()) {
                StartNav(true, online);
                DEBUG_OUT("enter " << (online ? "online" : "offline") << " nav mode");
            } else {
                ClearNavNodeIdRecord();
#ifdef AGV_DEBUG_NAV
                USHORT uAtNode = 100, utonode = 101;
                bool init_ok = SetInitStateByNode(uAtNode, utonode) == 0;
                if (init_ok) {
                    StartNav(true, online);
                    RCLCPP_INFO(rclcpp::get_logger("AgvNavigate"), "force set node %d; enter auto nav mode", static_cast<int>(uAtNode));
                }
#else
                RCLCPP_ERROR(rclcpp::get_logger("AgvNavigate"), "init pos is failed");
                TryToOccurPathLost("ready init pos is failed", true);
#endif
            }
        }
        PublicData.cur_work_mode = static_cast<AgvWorkMode>(new_work_mode);
        // #2 手动/半自动模式下,任务是取消状态
        if (PublicData.cur_work_mode == AgvWorkMode::mod_manual || PublicData.cur_work_mode == AgvWorkMode::mod_semiauto) {
            SetChargeIo(false);
            SetCancelTask(true);
            ClearIdentification();
        }
        DEBUG_OUT("Change Agv to newMode: " << new_work_mode << ", last mode: " << static_cast<int>(last_mode));
    }
}

/**
 * @brief 离线订单任务回调函数
 *
 * 当接收到新的离线订单任务时，此函数将被调用。
 *
 * @param req 请求参数，包含离线订单任务的信息
 * @param res 响应参数，用于返回操作结果
 * @return 如果操作成功，则返回true；否则返回false
 */
bool OfflineOrderTaskCallback(const agv_srvs::srv::LocalOrder::Request::SharedPtr request,
        agv_srvs::srv::LocalOrder::Response::SharedPtr response) {
    std::lock_guard<std::mutex> locker(g_mode_mutex);
    if (PublicData.cur_work_mode != AgvWorkMode::mod_auto_offline) {
        RCLCPP_WARN(rclcpp::get_logger("AgvNavigate"), "cur mode is not mod_auto_offline, cannot add new order task");
        std::string msg = "current mode is not auto_offline, cannot add new order task";
        response->success = false;
        response->message = msg;
        return true;
    }
    std::string time_stamp = request->order.timestamp;
    std::string agv_number = request->order.serialnumber;
    DEBUG_OUT("local order: agv_number: " << agv_number);
    bool ret = AddOfflineOrderTask(*request);
    response->success = ret;
    std::string msg = ret ? "ok" : "failed: order id is the same as last task";
    response->message = msg;
    return true;
}

/**
 * @brief 处理订单任务的回调函数
 *
 * 该函数用于处理订单任务，判断当前工作模式是否为自动在线模式，
 * 如果是，则根据请求添加订单任务；否则返回错误信息。
 *
 * @param req 请求参数，包含订单信息
 * @param res 响应参数，包含操作结果和消息
 * @return 返回true表示处理成功
 */
bool OrderTaskCallback(const agv_srvs::srv::Order::Request::SharedPtr request,
        agv_srvs::srv::Order::Response::SharedPtr response) {
    std::lock_guard<std::mutex> locker(g_mode_mutex);
    if (PublicData.cur_work_mode != AgvWorkMode::mod_auto_online) {
        DEBUG_WARN_OUT("cur mode is not mod_auto_online, cannot add new order task");
        std::string msg = "cur mode is not mod_auto_online, cannot add new order task";
        response->success = false;
        response->message = msg;
        return true;
    }
    std::string time_stamp = request->order.timestamp;
    std::string agv_number = request->order.serialnumber;
    DEBUG_WARN_OUT("OrderTaskCallback: agv_number: " << agv_number);
    bool ret = AddOnlineOrderTask(request->order);
    response->success = ret;
    std::string msg = ret ? "ok" : "failed: order id is the same as last task or orderId is not number";
    response->message = msg;
    return true;
}

/**
 * @brief 瞬时动作回调函数
 *
 * 处理来自外部的瞬时动作请求，并根据当前的工作模式和请求的来源进行相应的处理。
 *
 * @param req 请求参数，包含瞬时动作的来源和具体的瞬时动作列表
 * @param res 响应参数，包含操作结果和消息
 *
 * @return 如果处理成功返回true，否则返回false
 */
bool InstantActionCallback(const agv_srvs::srv::InstantActions::Request::SharedPtr request,
        agv_srvs::srv::InstantActions::Response::SharedPtr response) {
    std::lock_guard<std::mutex> locker(g_mode_mutex);
    bool local = ACTION_SOURCE == request->source;
    std::vector<agv_msgs::msg::Action> instant_actions = request->actions;
    DEBUG_OUT("instant action: source=" << request->source << ", local=" << local);

    if (local) {
        if (PublicData.cur_work_mode != AgvWorkMode::mod_auto_offline) {
            DEBUG_ERROR_OUT("cur mode is not mod_auto_offline, cannot add instant actions");
            return false;
        }
    } else {  // 如果是调度发送的小车控制消息，运行任何模式下执行
        if (instant_actions.size() > 0 && instant_actions[0].actiontype == "vehicleControl") {
            DEBUG_OUT("vehicleControl will execute; cur_work_mode=" << static_cast<int>(PublicData.cur_work_mode));
        } else { // 其他种类动作，必须在自动主机模式下才可执行
            if (PublicData.cur_work_mode != AgvWorkMode::mod_auto_online) {
                DEBUG_WARN_OUT("cur mode is not mod_auto_online, cannot add instant actions");
                return false;
            }
        }
    }

    bool ret = HandleInstantActions(instant_actions);
    response->message = ret ? "ok" : "failed";
    response->success = ret;
    return ret;
}

/**
 * @brief 获取版本号
 *
 * 从请求中获取版本号，并将版本号返回给响应
 *
 * @param req 请求对象，包含需要获取版本号的请求信息
 * @param res 响应对象，用于存储获取到的版本号信息
 * @return bool 函数执行成功返回true，否则返回false
 */
bool GetVersion(const std_srvs::srv::SetBool::Request::SharedPtr request, 
        std_srvs::srv::SetBool::Response::SharedPtr response) {
    static const char build[] = COMPLETE_TIME_STR();
    std::string version = "V" + std::to_string(VERSION_MAJOR) + "." + 
                            std::to_string(VERSION_MINOR) + "." + 
                            VERSION_HASH + " build@" + build;
    response->success = true;
    response->message = version;
    return true;
}

////////////////////静态op使用数据////////////////////////
/**
 * @brief Odometry消息回调函数
 *
 * 该函数是一个回调函数，用于处理从AGV（自动导引车）接收到的里程计消息。
 *
 * @param msg 里程计消息，类型为agv_msgs::msg::OdometryLite
 */
void OdomCallback(const agv_msgs::msg::OdometryLite& msg) {
    PublicData.op_data.odom_msg = msg;
    DEBUG_OUT("=== OdometryLite Data ===");
        
    // 输出 header 信息
    DEBUG_OUT("Header:");
    DEBUG_OUT("  seq:" << msg.header.stamp.sec 
                << "," << msg.header.stamp.nanosec);
    DEBUG_OUT("  frame_id:" <<
               msg.header.frame_id);
    
    // 输出 child_frame_id
    DEBUG_OUT("Child Frame ID:" << 
               msg.child_frame_id);
    
    // 输出 pose 信息
    DEBUG_OUT("Pose:");
    DEBUG_OUT("  Position: x=" << msg.pose.position.x  << 
                ", y=" << msg.pose.position.y << 
                ", z=" << msg.pose.position.z);
    DEBUG_OUT("  Orientation: x=" << msg.pose.orientation.x <<
                ", y=" << msg.pose.orientation.y <<
                ", z=" << msg.pose.orientation.z << 
                ", w=" << msg.pose.orientation.w);
    
    // 输出 twist 信息
    DEBUG_OUT("Twist:");
    DEBUG_OUT("  Linear: x=" << msg.twist.linear.x <<
                ", y=" << msg.twist.linear.y << 
                ", z=" << msg.twist.linear.z);
    DEBUG_OUT("  Angular: x=" << msg.twist.angular.x <<
                ", y=" << msg.twist.angular.y << 
                ", z=" << msg.twist.angular.z);  
    DEBUG_OUT("==============================");
}

/**
 * @brief 外设状态回调函数
 *
 * 当接收到外设状态消息时，调用此函数更新公共数据中的外设状态消息。
 *
 * @param msg 外设状态消息，类型为 agv_msgs::msg::PeriphStatus
 */
void PeriphStatusCallback(const agv_msgs::msg::PeriphStatus& msg) {
    PublicData.op_data.periph_status_msg = msg;
}

/**
 * @brief 处理模拟数据输入回调
 *
 * 该函数用于处理从AGV（自动导引车）接收到的模拟数据输入消息，并将该消息存储在全局变量中。
 *
 * @param msg agv_msgs::msg::ReadInPutsAnalog类型的消息，包含模拟输入数据
 */
void GetAnalogCallback(const agv_msgs::msg::ReadInPutsAnalog& msg) {
    PublicData.op_data.input_analog_msg = msg;
}

/**
 * @brief 读取新输入的回调函数
 *
 * 当收到新的输入消息时，该函数会被调用。
 *
 * @param msg 新的输入消息，类型为agv_msgs::msg::ReadInPutsNew
 */
void ReadInputNewCallback(const agv_msgs::msg::ReadInPutsNew& msg) {
    PublicData.op_data.input_new_io_msg.Update(msg);
}

/**
 * @brief 获取定位相机回调函数
 *
 * 当接收到定位相机位置信息时，此函数会被调用，并将接收到的位置信息保存到公共数据中。
 *
 * @param msg 包含定位相机位置信息的PoseStamped消息
 */
void GetGroundCameraCallback(const geometry_msgs::msg::PoseStamped& msg) {
    PublicData.op_data.ground_camera_pose_msg = msg;
}

/**
 * @brief 获取托盘相机姿态的回调函数
 *
 * 当接收到托盘相机姿态信息时，调用此函数更新全局数据中的托盘相机姿态信息。
 *
 * @param msg 包含托盘相机姿态信息的PoseStamped消息
 */
void GetRackCameraCallback(const geometry_msgs::msg::PoseStamped& msg) {
    PublicData.op_data.rack_camera_pose_msg = msg;
}

/**
 * @brief 停止位反馈回调函数
 *
 * 当接收到停止位消息时，更新全局数据中的停止位发生次数
 *
 * @param msg agv_msgs::msg::SetStopBits 类型的消息，包含停止位的信息
 */
void StopBitsFeedbackCallback(const agv_msgs::msg::SetStopBits& msg) {
    PublicData.stopBitOccur.Update(msg);
}


void GetPalletCallback(const agv_msgs::msg::RecognitionResult &msg)
{
    PublicData.op_data.pallet_result_msg.Update(msg);
}

/**
 * @brief TaskAndSubNavInfoThread 函数用于初始化并启动ROS节点，订阅相关话题和服务，用于处理任务与子导航信息
 *
 * @param nh ROS节点句柄指针
 */
void TaskAndSubNavInfoThread(rclcpp::Node::SharedPtr node) {
    DEBUG_OUT("TaskAndSubNavInfoThread: SingleCheckObj->Init start");
    // 初始开启二维码检测线程
    SingleCheckObj->Init();
    // 初始化半自动上线逻辑
    // PublicData.half_auto_line.Init();
    // op action 可能使用的数据
    auto odom_sub = node->create_subscription<agv_msgs::msg::OdometryLite>("/odom", 1, OdomCallback);
    auto periph_status_sub = node->create_subscription<agv_msgs::msg::PeriphStatus>("/periphStatus", 2, PeriphStatusCallback);
    auto io_sub = node->create_subscription<agv_msgs::msg::ReadInPutsNew>("/readInputNew", 1, ReadInputNewCallback);
    auto ground_camera_sub = node->create_subscription<geometry_msgs::msg::PoseStamped>("/landmark", 1, GetGroundCameraCallback);
    auto rack_camera_sub = node->create_subscription<geometry_msgs::msg::PoseStamped>("/landmark_shelf", 1, GetRackCameraCallback);
    auto analog_sub = node->create_subscription<agv_msgs::msg::ReadInPutsAnalog>("/readInputAnalog", 1, GetAnalogCallback);
    auto event_occur_sub = node->create_subscription<agv_msgs::msg::AGVEventStatus>("/eventstatus", 1, EventCallback);
    auto pose_qr_sub = node->create_subscription<agv_msgs::msg::PoseWithConfidence>("/pose", 2, PoseCallback);
    auto slam_score_sub = node->create_subscription<agv_msgs::msg::ConfidenceLevel>("/slam_confidence_score", 3, SlamScoreCallback);
    auto pallet_sub = node->create_subscription<agv_msgs::msg::RecognitionResult>("/pallet_result", 1, GetPalletCallback);  // 订阅托盘识别结果

    auto local_order_task_srv = node->create_service<agv_srvs::srv::LocalOrder>("/local_order", OfflineOrderTaskCallback);
    auto order_task_srv = node->create_service<agv_srvs::srv::Order>("/vda5050_order", OrderTaskCallback);
    auto instant_action_task_srv = node->create_service<agv_srvs::srv::InstantActions>("/vda5050_instantActions", InstantActionCallback);
    auto version_service = node->create_service<std_srvs::srv::SetBool>("/get_nav_version", GetVersion);

    auto agv_mode_sub = node->create_subscription<agv_msgs::msg::AGVMode>("/agvmode", 2, AgvModeCallback);
    auto steer_dir_sub = node->create_subscription<agv_msgs::msg::SteerDirFeedBack>("/steer_state", 2, SteerDirFeedbackCallback);
    auto stop_bits_sub = node->create_subscription<agv_msgs::msg::SetStopBits>("/set_stop_bits", 2, StopBitsFeedbackCallback);
    
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    DEBUG_OUT("TaskAndSubNavInfoThread:end");
}

/**
 * @brief 发布节点事件
 *
 * 发布节点事件到ROS主题。如果订阅者数量大于0，则发布事件。
 * 如果事件触发，则在50毫秒内发布所有事件。如果未触发事件，则在500毫秒内发布默认事件。
 *
 * @param pub ROS发布者对象
 */
static void PubNodeEvent(const std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::AGVEvent>> pub) {
    uint32_t num = ROSNode()->count_subscribers("/agvevent");
    bool valid = num > 0;
    if (!valid) {
        DEBUG_WARN_OUT("PubNodeEvent failed: pub is not valid: num=" << static_cast<int>(num));
        return;
    }

    if (PublicData.node_event.Occur()) {
        static TimeStamp stamp1;
        if (stamp1.TimeOut(50)) {
            std::vector<agv_msgs::msg::AGVEvent> event_list = PublicData.node_event.GetEvent();
            for (const auto& event : event_list) {
                pub->publish(event);
            }
            stamp1.Update();
        }
    } else {
        static TimeStamp stamp2;
        if (stamp2.TimeOut(500)) {
            pub->publish(PublicData.node_event.GetNormalEvent());
            stamp2.Update();
        }
    }
}

/**
 * @brief 记录并输出里程计数据
 *
 * 该函数会记录并输出AGV的里程计数据，包括实际速度和位置，以及控制输出的目标速度和角速度。
 */
static void LogOdomData() {
    auto real_odom_speed = PublicData.op_data.odom_msg.twist;
    auto real_odom_pos = PublicData.op_data.odom_msg.pose;
    try {
        DEBUG_OUT("agv odm real speed x: " << real_odom_speed.linear.x << ", y: " << real_odom_speed.linear.y
                                           << ", z: " << real_odom_speed.angular.z
                                           << ", realOdomPos: "
                                           << ", x=" << real_odom_pos.position.x
                                           << ", y=" << real_odom_pos.position.y
                                           << ", z=" << real_odom_pos.position.z
                                           << ", (degree)yaw=" << TO_DEGREE(Angle::NormAngle2(tf2::getYaw(real_odom_pos.orientation))));
    } catch (...) {
        DEBUG_ERROR_OUT("agv odm real speed or pos exception");
    }

    DEBUG_OUT("out target vel=" << PublicData.ctrl_out_vel.twist.linear.x << ", y=" << PublicData.ctrl_out_vel.twist.linear.y
                                << ", angVel=" << PublicData.ctrl_out_vel.twist.angular.z);
}

/**
 * @brief 记录AGV状态数据日志
 *
 * 该函数用于记录AGV状态数据的日志。如果指定的时间间隔内没有记录日志，则会记录一次。
 *
 * @param heart_beat AGV状态数据，类型为agv_msgs::msg::State
 */
static void LogMsgStateData(const agv_msgs::msg::State& heart_beat) {
    static TimeStamp flag;
    if (flag.TimeOut(500)) {
        flag.Update();
        // DEBUG_OUT("heartBeat=" << heart_beat);
    }
}

/**
 * @brief 发布导航速度和状态信息的线程函数
 *
 * 该函数用于在一个独立的线程中发布AGV的导航速度和状态信息。
 *
 * @param nh ROS节点句柄指针，用于与ROS系统交互
 */
void PublishNavVelAndStatusThread(rclcpp::Node::SharedPtr node) {
    AgvVelPub = node->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel", 5);
    auto event_pub = node->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 1);
    PublicData.ctrl_out_vel.twist.linear.x = 0;
    PublicData.ctrl_out_vel.twist.linear.y = 0;
    PublicData.ctrl_out_vel.twist.angular.z = 0;
    DEBUG_OUT("agv_navigate set start ok");
    rclcpp::Rate rate(50);
    while (rclcpp::ok()) {
        bool b_online = PublicData.cur_work_mode == AgvWorkMode::mod_auto_online;
        bool b_offline = PublicData.cur_work_mode == AgvWorkMode::mod_auto_offline;
        if (b_online || b_offline) {
            LogOdomData();
            AgvVelPub->publish(PublicData.ctrl_out_vel);
        }
        PubNodeEvent(event_pub);
        rate.sleep();
    }
}

/**
 * @brief 自动状态线程函数
 *
 * 该函数用于在ROS节点中发布AGV的自动运行状态消息。
 *
 * @param nh ROS节点句柄指针
 */
void AutoStatusThread(rclcpp::Node::SharedPtr node) {
    agv_msgs::msg::AgvAutoStatus msg;
    auto auto_status_pub = node->create_publisher<agv_msgs::msg::AgvAutoStatus>("auto_run_status", 1);
    rclcpp::Rate rate(20);
    while (rclcpp::ok()) {
        msg.taskid = VhcStatus.task_id_;
        msg.subtaskid = VhcStatus.sub_task_id_;
        msg.fromnode = VhcStatus.from_node_;
        msg.tonode = VhcStatus.to_node_;
        msg.fprogress = VhcStatus.abs_progress_;
        msg.fprogpercent = VhcStatus.prog_percent_;

        auto to_rcs = VhcStatus.rcs_fb_;
        msg.orderidnew = to_rcs.order_id;
        msg.orderupdateidnew = to_rcs.order_update_id;
        msg.nodeidnew = to_rcs.node_id;

        float fx = 0, fy = 0, ft = 0, fq = 0;
        GetLaserAutoPos(fx, fy, ft, fq, false, true);
        msg.curpst_x = fx;
        msg.curpst_y = fy;
        msg.curpst_t = Angle::NormAngle2(ft);
        msg.fquality = fq;
        msg.curvel_x = VhcStatus.velocity_.linear_;
        msg.curvel_z = VhcStatus.velocity_.angular_;
        msg.vellevel = VhcStatus.level_;
        msg.fdev = VhcStatus.dev_x_;
        msg.fdevprog = VhcStatus.deviation_progress_;
        msg.fdevthita = TO_DEGREE(Angle::NormAngle2(VhcStatus.dev_theta_));
        msg.event = VhcStatus.event_;
        msg.workstate = VhcStatus.work_state_;
        msg.wheelmovedir = static_cast<unsigned char>(VhcStatus.wheel_move_dir_.move_dir_tag_);
        msg.trajtype = VhcStatus.traj_type_;
        msg.locatetype = static_cast<int>(VhcStatus.loc_type_);
        msg.rackfollowup = VhcStatus.rack_follow_up_;
        msg.navigationtype = GetCurNavType();
        msg.startdelay = VhcStatus.to_run_blink_;

        msg.driving = !VhcStatus.is_idle_;
        msg.paused = VhcStatus.is_idle_ ? false : (VhcStatus.mode_ != PROFILE_MODE_NORMAL && VhcStatus.mode_ != PROFILE_MODE_REFRAIN);
        msg.idle = VhcStatus.is_idle_;

        RunPathProperty property = GetRunPathProperty();
        msg.scenes = property.scenes;
        msg.bits = property.bits;
        msg.word1 = property.word1;
        msg.word2 = property.word2;
        msg.pathword1 = property.path_word1;
        msg.pathword2 = property.path_word2;

        auto_status_pub->publish(msg);
        rate.sleep();
    }
}

/**
 * @brief AGV在线状态发布线程函数
 *
 * 该函数启动一个线程，用于定期发布AGV的在线状态信息。
 *
 * @param nh ROS节点句柄指针
 */
void PubOnlineStateThreadFun(rclcpp::Node::SharedPtr node) {
    auto v_pub_state = node->create_publisher<agv_msgs::msg::State>("vda5050_state", 5);
    rclcpp::Rate rate(10);
    while (rclcpp::ok()) {
        agv_msgs::msg::State state;
        state.orderid = std::to_string(VhcStatus.task_id_);
        state.orderupdateid = VhcStatus.sub_task_id_;
        state.lastnodeid = std::to_string(VhcStatus.from_node_);
        state.lastnodesequenceid = 0;
        state.driving = !VhcStatus.is_idle_;
        // 修复bug; 当接近路线结束时，触发避障等事件，进入挂起模式，未清除VhcStatus.m_nMode标志
        bool op = VhcStatus.work_state_ == AGV_IS_IN_OP;
        state.paused = (VhcStatus.is_idle_ || op) ? false : (VhcStatus.mode_ != PROFILE_MODE_NORMAL);
        state.distancesincelastnode = VhcStatus.abs_progress_;
        state.agvposition.mapid = VhcStatus.map_id_;

        float fx = 0, fy = 0, ft = 0, fq = 0;
        GetLaserAutoPos(fx, fy, ft, fq, false, false);
        state.agvposition.x = fx;
        state.agvposition.y = fy;
        state.agvposition.theta = Angle::NormAngle2(ft);
        state.agvposition.localizationscore = fq;

        state.velocity.vx = VhcStatus.velocity_.linear_;
        state.velocity.vy = 0;
        state.velocity.omega = VhcStatus.velocity_.angular_;

        state.actionstates = PublicData.cur_action_state_list.states_;
        std::vector<agv_msgs::msg::EdgeState> cur_edges;
        cur_edges.push_back(VhcStatus.FindCurEdge());
        state.edgestates = cur_edges;

        agv_msgs::msg::Load load_state;
        int virtual_load = ROSNode()->get_parameter_or("virtualLoad", 0);
        load_state.loadposition = std::to_string(virtual_load);
        std::vector<agv_msgs::msg::Load> loads;
        loads.push_back(load_state);
        state.loads = loads;

        v_pub_state->publish(state);
        LogMsgStateData(state);
        rate.sleep();
    }
}

/////////////////其他辅助类工具函数/////////////////////////
/**
 * @brief 判断数据是否超时
 *
 * 判断给定时间戳 `data_time` 是否距离当前时间 `cur_time` 超过 `max_time_ms` 毫秒。
 *
 * @param data_time 需要判断的时间戳
 * @param cur_time 当前时间戳
 * @param max_time_ms 最大允许的时间差（毫秒）
 *
 * @return 如果 `data_time` 超过 `max_time_ms` 毫秒，则返回 `true`，否则返回 `false`
 */
bool IsTimeOut(const rclcpp::Time& data_time, const rclcpp::Time& cur_time, const double& max_time_ms) {
#ifdef AGV_DEBUG_NAV
    return false;
#endif
    double time_diff_ms = (cur_time - data_time).seconds() * 1000.0;
    bool ret = time_diff_ms > max_time_ms;
    if (ret) {
        DEBUG_ERROR_OUT("data time out: time_diff_ms=" << time_diff_ms << ", max_time_ms=" << max_time_ms);
    }
    return ret;
}

/**
 * @brief 获取AGV载货状态
 *
 * 从操作数据中获取AGV的载货状态，并返回给调用者。
 *
 * @param b_load 用于存储获取的载货状态
 * @return 如果成功获取到载货状态，则返回true；如果超时，则返回false
 */
bool GetAgvLoad(bool& b_load) {
    const auto& op_data = GetOpData();
    auto perph_status = op_data.periph_status_msg;

    if (IsTimeOut(perph_status.header.stamp, ROSTime(), 500)) {
        DEBUG_ERROR_OUT("get agv load: time out");
        return false;
    }
    b_load = perph_status.loaded;
    return true;
}
    
/**
 * @brief 设置载货标志位
 *
 * 读取开放载货速度参数，并根据标志位状态获取AGV载货状态，然后更新载货标志位。
 *
 * @return bool 返回当前载货状态，true 表示载货中，false 表示未载货
 */
bool SetLoadFlags() {
    MotionParm::ReadOpenLoadVelParam();
    auto& flags = MotionParm::load_flags_;
    bool ret = false;
    bool load = false;
    if (flags.open) {
        ret = GetAgvLoad(load);
    }
    flags.current_load = load;
    DEBUG_OUT("SetLoadFlags: flags.open=" << static_cast<int>(flags.open)
                                          << " GetAgvLoad: ret=" << static_cast<int>(ret)
                                          << ", load=" << static_cast<int>(load));
    return load;
}



} // end extern "C"
